#include "camera_opencv_receiver.h"

#include <unistd.h>

#include <linux/videodev2.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <sys/types.h>

using namespace sensor::camera;

CameraOpencvReceiver::CameraOpencvReceiver(std::string dev, muduo::net::EventLoop* loop,
    size_t width, size_t height, const std::string& fmt)
    : CameraReceiver("CameraOpencvReceiver", dev, loop, width, height, fmt)
{
}

bool CameraOpencvReceiver::prepareBuffers()
{
    struct v4l2_requestbuffers requestbuffers;
    memset(&requestbuffers, 0x00, sizeof(requestbuffers));
    requestbuffers.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    requestbuffers.count = buffers_.size(); // 申请缓冲区--不超过5
    requestbuffers.memory = V4L2_MEMORY_MMAP;
    if (ioctl(chnl_->fd(), VIDIOC_REQBUFS, &requestbuffers) < 0) {
        RCLCPP_ERROR(logger_, "device %s ioctl VIDIOC_REQBUFS failure! %s", video_dev_.c_str(), strerror(errno));
        return false;
    }

    if (requestbuffers.count != buffers_.size()) {
        RCLCPP_ERROR(logger_, "V4l2 buffer number is not as desired");
        return false;
    }

    for (size_t i = 0; i < buffers_.size(); i++) {
        struct v4l2_buffer buffer;
        memset(&buffer, 0x00, sizeof(buffer));
        buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buffer.index = i;

        if (ioctl(chnl_->fd(), VIDIOC_QUERYBUF, &buffer) < 0) {
            RCLCPP_ERROR(logger_, "device %s ioctl VIDIOC_QUERYBUF %d/%d failure! %s",
                video_dev_.c_str(), (i + 1), buffers_.size(), strerror(errno));
            return false;
        }

        // 映射
        buffers_[i].maddr = (unsigned char*)mmap(nullptr, buffer.length, PROT_READ | PROT_WRITE, MAP_SHARED,
            chnl_->fd(), buffer.m.offset);
        buffers_[i].msize = buffer.length;

        if (MAP_FAILED == buffers_[i].maddr) {
            RCLCPP_ERROR(logger_, "device %s mmap %d/%d failure! %s", video_dev_.c_str(), (i + 1),
                buffers_.size(), strerror(errno));
            return false;
        }

        // 放回队列
        if (ioctl(chnl_->fd(), VIDIOC_QBUF, &buffer) < 0) {
            RCLCPP_ERROR(logger_, "device %s ioctl VIDIOC_QBUF %d/%d failure! %s",
                video_dev_.c_str(), (i + 1), buffers_.size(), strerror(errno));
            return false;
        }
    }

    return true;
}

void CameraOpencvReceiver::releaseBuffers()
{
    for (size_t i = 0; i < buffers_.size(); i++) {
        if (MAP_FAILED == buffers_[i].maddr) {
            continue;
        }
        munmap(buffers_[i].maddr, buffers_[i].msize);
    }
}

void CameraOpencvReceiver::onReceiveOneFrame()
{
    if (counter_++ % 2 == 0) {
        struct v4l2_buffer buffer;
        memset(&buffer, 0x00, sizeof(buffer));
        buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        if (ioctl(chnl_->fd(), VIDIOC_DQBUF, &buffer) < 0) {
            RCLCPP_ERROR(logger_, "device %s ioctl VIDIOC_DQBUF failure! %s", video_dev_.c_str(), strerror(errno));
            return;
        }

        // RCLCPP_INFO(logger_, "device %s convert begin", video_dev_.c_str());
        // frame->image = convertor_.convert_uyvy422_to_bgr_by_cuda(buffers_[buffer.index].maddr, width_, height_);
        // frame->image = convertor_.convert_uyvy422_to_bgr_by_opencv(buffers_[buffer.index].maddr, width_, height_);
        // RCLCPP_INFO(logger_, "device %s convert done", video_dev_.c_str());
        // cv::resize(frame->image, frame->image, cv::Size(640, 360));

        if (ioctl(chnl_->fd(), VIDIOC_QBUF, &buffer) < 0) {
            RCLCPP_ERROR(logger_, "device %s ioctl VIDIOC_QBUF failure! %s", video_dev_.c_str(), strerror(errno));
        }
        return;
    }

    auto frame = std::make_shared<CameraOneFrame>();

    struct v4l2_buffer buffer;
    memset(&buffer, 0x00, sizeof(buffer));
    buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    if (ioctl(chnl_->fd(), VIDIOC_DQBUF, &buffer) < 0) {
        RCLCPP_ERROR(logger_, "device %s ioctl VIDIOC_DQBUF failure! %s", video_dev_.c_str(), strerror(errno));
        return;
    }

    // RCLCPP_INFO(logger_, "device %s convert begin", video_dev_.c_str());
    // frame->image = convertor_.convert_uyvy422_to_bgr_by_cuda(buffers_[buffer.index].maddr, width_, height_);
    frame->image = convertor_.convert_uyvy422_to_bgr_by_opencv(buffers_[buffer.index].maddr, width_, height_);
    // RCLCPP_INFO(logger_, "device %s convert done", video_dev_.c_str());
    cv::resize(frame->image, frame->image, cv::Size(640, 360));

    if (ioctl(chnl_->fd(), VIDIOC_QBUF, &buffer) < 0) {
        RCLCPP_ERROR(logger_, "device %s ioctl VIDIOC_QBUF failure! %s", video_dev_.c_str(), strerror(errno));
    }

    if (cb_) {
        cb_(frame);
    }
}
